//
// Texas Instruments (TI) is supplying this software for use solely and
// exclusively on TI's microcontroller products. The software is owned by
// TI and/or its suppliers, and is protected under applicable copyright
// laws. You may not combine this software with "viral" open-source
// software in order to form a larger program.
//
// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS.
// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT
// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY
// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
// DAMAGES, FOR ANY REASON WHATSOEVER.
//
// This is part of revision 9453 of the EK-LM4F120XL Firmware Package.
//
//*****************************************************************************

#define UART_BUFFERED

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_ints.h"
#include "inc/hw_timer.h"

#include "driverlib/debug.h"
#include "driverlib/fpu.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/rom.h"
#include "driverlib/sysctl.h"
#include "driverlib/i2c.h"
#include "utils/uartstdio.h"
#include "driverlib/rom_map.h"
#include "driverlib/timer.h"

#include "GY80.h"
#include "my_i2c.h"
#include "kalman.h"
#include "PID.h"
#include "math.h"
#include "motorHAL.h"
#include "FIR.h"

//*****************************************************************************
//
// The error routine that is called if the driver library encounters an error.
//
//*****************************************************************************
#ifdef DEBUG
void
__error__(char *pcFilename, unsigned long ulLine)
{
}
#endif

// Updates Per Second
#define UPS 100

void MainLoop(void);

volatile char semaphore = 0;

sKalmanFilter kalmanfilter;
sPID regulator;
sSensors sensors;

#define cP 		6.0
#define cI 		32.0
//
#define cD 		0.0

volatile float cAngleOffset = -8.0;

float sterowanie = 0;

extern int motor_dead_zone;
volatile int testmode = 0;

// Periodic sensors update
void TimerTick(void)
{
	//if (semaphore == 1)
	//	UARTprintf("[Warning! Overlap]");

	TimerIntClear(TIMER0_BASE, INT_TIMER0A);

	GY80_Update(&sensors);

	semaphore = 1;
}

int main(void)
{
	volatile unsigned char led = 1;
	float angle, angle_XY;
	sFIR sfir;

	FIR_Init(&sfir);

	ROM_FPULazyStackingEnable ();

	//
	// Set the clocking to run directly from the crystal.
	//
	ROM_SysCtlClockSet (SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

	// Pin for powering GY80 (reset)
	ROM_SysCtlPeripheralEnable (SYSCTL_PERIPH_GPIOB);
	ROM_GPIOPinTypeGPIOOutput (GPIO_PORTB_BASE, GPIO_PIN_5);

	//
	// init LED RGB
	//
	ROM_SysCtlPeripheralEnable (SYSCTL_PERIPH_GPIOF);
	ROM_GPIOPinTypeGPIOOutput (GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3);
	ROM_GPIOPinWrite (GPIO_PORTF_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3, 0x00);

	//
	// Initialize the UART.
	//
	ROM_SysCtlPeripheralEnable (SYSCTL_PERIPH_GPIOA);
	ROM_GPIOPinConfigure (GPIO_PA0_U0RX);
	ROM_GPIOPinConfigure (GPIO_PA1_U0TX);
	ROM_GPIOPinTypeUART (GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
	UARTStdioInit(0);
	UARTprintf("[HELLO]\n");

	//
	// init i2c
	//
	i2c_init();

	// activate gy-80
	//ROM_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_5, 0);
	//ROM_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_5, GPIO_PIN_5);

	//
	// enable interrupts
	//
	ROM_IntMasterEnable ();

	//
	// init sensor board
	//

	GY80_init(&sensors);

	//
	// init filter
	//
	Kalman_Init(&kalmanfilter);

	//
	// init regulator
	//
	PID_Initialize(&regulator, cP, cI, cD, 1.0 / UPS);

	//
	// init periodic timer
	//
	SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
	TimerConfigure(TIMER0_BASE, TIMER_CFG_A_PERIODIC);
	TimerLoadSet(TIMER0_BASE, TIMER_BOTH, SysCtlClockGet() / UPS);
	TimerControlEvent(TIMER0_BASE, TIMER_0A_SYNC, TIMER_EVENT_BOTH_EDGES);
	TimerEnable(TIMER0_BASE, TIMER_0A_SYNC);
	TimerIntEnable(TIMER0_BASE, INT_TIMER0A);
	ROM_IntEnable (INT_TIMER0A);

	//
	// Motor pwm
	//
	motor_init();

	UARTprintf("[READY]\n");

	//i2c_perform_scan();

	// enter main loop

	UARTprintf("[LOOP]\n");
	for (;;)
	{
		// if measurement was performed
		if (semaphore)
		{
			angle_XY = GY80_ReadAngleAccZX(&sensors);

			angle = Kalman_Process(&kalmanfilter, angle_XY, 14.0 * sensors.gY, UPS) + cAngleOffset;

			sterowanie = PID_Process(&regulator, angle);

			sterowanie = FIR_Get(&sfir, sterowanie);

			UARTprintf("~,%4d,%4d,%4d,%4d\n", (int) (angle_XY * 1000), (int) (sensors.gY * 1000), (int) (angle * 1000), (int) (sterowanie * 1000));

			motor_set(sterowanie);

			sterowanie = 0;

			semaphore = 0;
		}

		if (UARTRxBytesAvail())
		{
			switch (UARTgetc())
			{
			case 'd':
				//kalmanfilter.R_measure += 0.1;
				regulator.P += 0.1;
				break;
			case 'c':
				//kalmanfilter.R_measure -= 0.1;
				regulator.P -= 0.1;
				break;
			case 'f':
				//kalmanfilter.Q[0] += 1;
				regulator.I += 0.1;
				break;
			case 'v':
				//kalmanfilter.Q[0] = 1;
				regulator.I -= 0.1;
				break;
			case 'g':
				//kalmanfilter.Q[1] += 1;
				regulator.D += 0.1;
				break;
			case 'b':
				//kalmanfilter.Q[1] -= 1;
				regulator.D -= 0.1;
				break;
			case 'k':
				cAngleOffset += 0.05;
				break;
			case 'l':
				cAngleOffset -= 0.05;
				break;
			case 'q':
				motor_dead_zone += 10;
				break;
			case 'w':
				motor_dead_zone -= 10;
				break;
			case 'r':
				PID_Reset(&regulator);
				break;
			case 'e':
				testmode += 1;
				UARTprintf("T,%i", testmode);
				break;
			case '\n':
				break;
			default:
				UARTprintf("!UC\n");
			}

		}
		UARTFlushTx(false);

	}
}
